#include <ros/ros.h>
#include "mcu_correspond/mcu_data_subscriber.h"
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <cmath>
#include <locale>
#include <algorithm>
#include "my_pcl/my_pcl.h"

// 全局变量存储杆子位置
double pole1_x = 0.0;  // 杆1的x坐标
double pole1_y = 0.0;  // 杆1的y坐标
double pole2_x = 0.0;  // 杆2的x坐标
double pole2_y = 0.0;  // 杆2的y坐标
bool pole1_found = false;  // 杆1是否找到
bool pole2_found = false;  // 杆2是否找到
bool pole1_stored = false;  // 杆1位置是否已存储
bool pole2_stored = false;  // 杆2位置是否已存储
bool pole1_published = false;  // 杆1位置是否已发布
bool pole2_published = false;  // 杆2位置是否已发布

// 杆子参数配置
const double STICK_MIN_DIAMETER = 0.03;  // 3cm - 最小直径
const double STICK_MAX_DIAMETER = 0.20;  // 12cm - 最大直径（针对5cm直径杆子放宽范围）
const double min_length = 0.3;  // 最小高度 30cm

// 简单的杆子信息结构
struct SimplePoleInfo {
    double x_position;
    double y_position;
    double diameter;
    double height;
    double distance;
};

// 找到多根杆子的函数 - 直接在聚类上工作，不依赖StickInfo
std::vector<SimplePoleInfo> findMultiplePoles(
    const std::vector<pcl::PointCloud<PointType>::Ptr>& merged_clusters,
    double min_diameter, double max_diameter, double min_height)
{
    std::vector<SimplePoleInfo> poles;
    
    ROS_INFO("========== 边界框分析开始 ==========");
    ROS_INFO("检测参数: 直径范围 %.1f-%.1fcm, 最小高度 %.1fcm", 
             min_diameter*100, max_diameter*100, min_height*100);
    ROS_INFO("总聚类数: %zu", merged_clusters.size());

    int cluster_index = 0;
    for (const auto& cluster : merged_clusters)
    {
        // 快速边界框计算
        PointType min_pt, max_pt;
        pcl::getMinMax3D(*cluster, min_pt, max_pt);

        double diameter = std::sqrt(std::pow(max_pt.x - min_pt.x, 2) + 
                                   std::pow(max_pt.y - min_pt.y, 2));
        double height = max_pt.z - min_pt.z;
        double width_x = max_pt.x - min_pt.x;
        double width_y = max_pt.y - min_pt.y;
        
        // 输出边界框详细信息
        ROS_INFO("聚类 %d 边界框信息:", cluster_index);
        ROS_INFO("  最小点: (%.3f, %.3f, %.3f)", min_pt.x, min_pt.y, min_pt.z);
        ROS_INFO("  最大点: (%.3f, %.3f, %.3f)", max_pt.x, max_pt.y, max_pt.z);
        ROS_INFO("  尺寸: X=%.3f, Y=%.3f, Z=%.3f", width_x, width_y, height);
        ROS_INFO("  计算直径: %.3f (%.1fcm)", diameter, diameter*100);
        ROS_INFO("  点云大小: %zu points", cluster->points.size());
        
        if (diameter >= min_diameter && diameter <= max_diameter && height > min_height)
        {
            Eigen::Vector4f centroid;
            pcl::compute3DCentroid(*cluster, centroid);
            double distance = std::sqrt(centroid[0]*centroid[0] + centroid[1]*centroid[1]);

            SimplePoleInfo pole;
            pole.x_position = centroid[0];
            pole.y_position = centroid[1];
            pole.diameter = diameter;
            pole.height = height;
            pole.distance = distance;
            
            poles.push_back(pole);
            ROS_INFO("  ✓ 符合杆子条件! 质心: (%.3f, %.3f), 距离: %.3f", 
                     centroid[0], centroid[1], distance);
        }
        else
        {
            ROS_INFO("  ✗ 不符合条件: 直径=%.1fcm (范围:%.1f-%.1fcm), 高度=%.1fcm (需要>%.1fcm)", 
                     diameter*100, min_diameter*100, max_diameter*100, height*100, min_height*100);
        }
        ROS_INFO("  ---");
        cluster_index++;
    }
    
    // 按距离排序，选择最近的杆子
    std::sort(poles.begin(), poles.end(), 
              [](const SimplePoleInfo& a, const SimplePoleInfo& b) {
                  return a.distance < b.distance;
              });
    
    ROS_INFO("========== 边界框分析结束 ==========");
    ROS_INFO("符合条件的杆子数量: %zu/%zu", poles.size(), merged_clusters.size());
    
    return poles;
}

class PoleDetector {
private:
    ros::NodeHandle nh_;
    McuDataSubscriber mcu_data_;

    // 点云订阅器
    ros::Subscriber pointcloud_sub_;
    ros::Publisher detected_sticks_pub_;    // 发布检测到的杆子点云

public:
    PoleDetector(ros::NodeHandle& nh) : nh_(nh), mcu_data_(nh) {
        // 订阅点云数据
        pointcloud_sub_ = nh_.subscribe("/livox/map", 1, &PoleDetector::pointCloudCallback, this);
        // pointcloud_sub_ = nh_.subscribe("/livox/PointXYZI", 1, &PoleDetector::pointCloudCallback, this);
        
        // 创建杆子检测发布器
        detected_sticks_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/detected_sticks", 1);
        
        ROS_INFO("Dual Pole Detection System initialized. Min diameter: %.1fcm, Max diameter: %.1fcm", 
                 STICK_MIN_DIAMETER*100, STICK_MAX_DIAMETER*100);
        ROS_INFO("Publishing topics: /clusters_cloud, /detected_sticks");
    }
    
    // 发布检测到的杆子点云
    void publishDetectedSticks(const std::vector<SimplePoleInfo>& poles,
                              const std::vector<pcl::PointCloud<PointType>::Ptr>& clusters,
                              const std_msgs::Header& header) {
        if (poles.empty()) return;
        
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr stick_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
        
        // 为每个检测到的杆子分配颜色
        std::vector<std::array<uint8_t, 3>> stick_colors = {
            {255, 0, 0},    // 杆1 - 红色
            {0, 255, 0},    // 杆2 - 绿色
            {0, 0, 255},    // 杆3 - 蓝色
            {255, 255, 0}   // 其他 - 黄色
        };
        
        for (size_t pole_idx = 0; pole_idx < poles.size(); ++pole_idx) {
            const auto& pole = poles[pole_idx];
            auto color = stick_colors[std::min(pole_idx, stick_colors.size()-1)];
            
            // 找到对应的聚类点云
            for (const auto& cluster : clusters) {
                // 计算聚类质心
                Eigen::Vector4f centroid;
                pcl::compute3DCentroid(*cluster, centroid);
                
                // 检查是否匹配当前杆子（通过位置匹配）
                double pos_diff = std::sqrt(std::pow(centroid[0] - pole.x_position, 2) + 
                                          std::pow(centroid[1] - pole.y_position, 2));
                
                if (pos_diff < 0.05) { // 5cm容差
                    for (const auto& point : cluster->points) {
                        pcl::PointXYZRGB colored_point;
                        colored_point.x = point.x;
                        colored_point.y = point.y;
                        colored_point.z = point.z;
                        colored_point.r = color[0];
                        colored_point.g = color[1];
                        colored_point.b = color[2];
                        stick_cloud->points.push_back(colored_point);
                    }
                    break;
                }
            }
        }
        
        stick_cloud->width = stick_cloud->points.size();
        stick_cloud->height = 1;
        stick_cloud->is_dense = true;
        
        // 转换为ROS消息并发布
        sensor_msgs::PointCloud2 sticks_msg;
        pcl::toROSMsg(*stick_cloud, sticks_msg);
        sticks_msg.header = header;
        detected_sticks_pub_.publish(sticks_msg);
    }
    
    // 检测并更新杆子位置
    void detectAndUpdatePoles(const sensor_msgs::PointCloud2ConstPtr& cloud_msg) {
        try {
            // 点云处理 - 使用ObjectFind库
            pcl::PointCloud<PointType>::Ptr cloud(new pcl::PointCloud<PointType>);
            pcl::fromROSMsg(*cloud_msg, *cloud);
            
            // 使用深度优化的ObjectFind库处理点云并发布彩色聚类
            auto clusters = processCloudsForStickDetection(cloud, "/clusters_cloud", cloud_msg->header.frame_id);
            
            // 找到所有符合条件的杆子
            std::vector<SimplePoleInfo> poles = findMultiplePoles(clusters, STICK_MIN_DIAMETER, STICK_MAX_DIAMETER, min_length);
            
            // 发布检测到的杆子点云
            publishDetectedSticks(poles, clusters, cloud_msg->header);
            
            // 重置杆子检测状态（但不重置已存储的位置）
            pole1_found = false;
            pole2_found = false;
            
            if (!poles.empty()) {
                // 按y坐标排序：y大的是杆1，y小的是杆2
                std::sort(poles.begin(), poles.end(), 
                          [](const SimplePoleInfo& a, const SimplePoleInfo& b) {
                              return a.y_position > b.y_position;  // y大的排在前面
                          });
                
                // 只有在杆1位置未存储时才更新杆1信息
                if (poles.size() >= 1 && !pole1_stored) {
                    pole1_x = poles[0].x_position;
                    pole1_y = poles[0].y_position;
                    pole1_found = true;
                    pole1_stored = true;  // 标记杆1位置已存储
                    ROS_INFO("杆1位置已锁定: x=%.3f, y=%.3f, 距离=%.3f, 直径=%.1fcm", 
                             pole1_x, pole1_y, poles[0].distance, poles[0].diameter*100);
                } else if (pole1_stored) {
                    pole1_found = true;  // 保持found状态以便发布
                }
                
                // 只有在杆2位置未存储时才更新杆2信息
                if (poles.size() >= 2 && !pole2_stored) {
                    pole2_x = poles[1].x_position;
                    pole2_y = poles[1].y_position;
                    pole2_found = true;
                    pole2_stored = true;  // 标记杆2位置已存储
                    ROS_INFO("杆2位置已锁定: x=%.3f, y=%.3f, 距离=%.3f, 直径=%.1fcm", 
                             pole2_x, pole2_y, poles[1].distance, poles[1].diameter*100);
                } else if (pole2_stored) {
                    pole2_found = true;  // 保持found状态以便发布
                }
                
                // 显示存储状态
                if (pole1_stored && pole2_stored) {
                    ROS_INFO("两个杆子位置已全部锁定，不再更新位置");
                } else {
                    ROS_INFO("检测到 %zu 根杆子，等待锁定剩余杆子", poles.size());
                }
            } else {
                if (!pole1_stored || !pole2_stored) {
                    ROS_WARN("未检测到符合条件的杆子，继续搜索...");
                }
            }
            
        } catch (const std::exception& e) {
            ROS_ERROR("杆子检测错误: %s", e.what());
        }
    }
    
    // 发送杆子位置数据
    void publishPoleData() {
        // 获取当前任务状态
        std::string currentStepName = mcu_data_.getStringValue("state");
        
        // 在wait_for_pole1状态时发送杆1位置（仅发布一次）
        if (pole1_found && !pole1_published) {
            mcu_data_.publishStringData("switch", "to1");
            mcu_data_.publishDoubleData("pole1_x", pole1_x);
            mcu_data_.publishDoubleData("pole1_y", pole1_y);
            pole1_published = true;  // 标记已发布
            ROS_INFO("杆1位置已发布: x=%.3f, y=%.3f (仅发布一次)", pole1_x, pole1_y);
        }
        // 在wait_for_pole2状态时发送杆2位置（仅发布一次）
        else if (pole2_found && !pole2_published) {
            mcu_data_.publishStringData("switch", "to2");
            mcu_data_.publishDoubleData("pole2_x", pole2_x);
            mcu_data_.publishDoubleData("pole2_y", pole2_y);
            pole2_published = true;  // 标记已发布
            ROS_INFO("杆2位置已发布: x=%.3f, y=%.3f (仅发布一次)", pole2_x, pole2_y);
        }
    }
    
    // 点云回调函数
    void pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg) {
        // 持续检测并更新杆子位置
        detectAndUpdatePoles(cloud_msg);
        
        // 根据状态发送相应的杆子数据
        publishPoleData();
    }
};

int main(int argc, char** argv) {
    std::setlocale(LC_ALL, "zh_CN.UTF-8");

    ros::init(argc, argv, "s2020basic");
    ros::NodeHandle nh;
    
    // Create pole detector
    PoleDetector detector(nh);
    
    ROS_INFO("Dual Pole Detection System started - Continuous monitoring enabled");
    ROS_INFO("Pole assignment: Pole1 (higher y-value), Pole2 (lower y-value)");
    ros::spin();
    return 0;
}
